#include "BeagleThread.h"
#include "Motor.h"
#include "WindSensor.h"
#include "Compass.h"

#define isSailPosition(x) ((x >= 0) && (x <= 90))
#define isRudderPosition(x) ((x >= 0) && (x <= 70))

BeagleThread::BeagleThread(Motor* main, Motor* jib, Motor* portRudder, Motor* stbdRudder, WindSensor* windSensor){
    _main = main;
    _jib = jib;
    _portRudder = portRudder;
    _stbdRudder = stbdRudder;

    _windSensor = windSensor;
    //_compass = compass;
    _lastAutonomyState = enableAutonomy;
}

void BeagleThread::run(){
    //Serial.println("b");

    byte buffer[20];
    size_t checksum = 0;

    bool check = true;
    while(check){
        if(Serial2.available() > 0){
            char c = Serial2.read();
            if(c == 'm'){
                //Serial.println("Beagle start byte");
                checksum = 0;
                for(size_t i=0; i<3; i++){
                    buffer[i] = static_cast<byte>(Serial2.read());
                    checksum += static_cast<size_t>(buffer[i]);
                    //Serial.println(buffer[i]);
                }
                buffer[3] = Serial2.read();
                buffer[4] = Serial2.read();

                size_t checksumRec = static_cast<size_t>(buffer[3]) + (static_cast<size_t>(buffer[4]) << 8);

                char recBuf[5];
                itoa(checksumRec, recBuf, 10);
                //Serial.println(recBuf);

                char buf[5];
                itoa(buffer[2], buf, 10);
                //Serial.println(buf);

                if((checksum == checksumRec) && isSailPosition(buffer[0]) && isSailPosition(buffer[1]) && isRudderPosition(buffer[2])){
                    digitalWrite(12, HIGH);
                    //Serial.println("passed");

                    int16_t mainPosition = static_cast<int16_t>(buffer[0]);
                    int16_t jibPosition = static_cast<int16_t>(buffer[1]);
                    int16_t rudderPosition = static_cast<int16_t>(buffer[2]);

                    _main->setCommandedPosition(mainPosition);
                    _jib->setCommandedPosition(jibPosition);
                    _portRudder->setCommandedPosition(rudderPosition-35); //Beagle sends rudder values 0-70
                    _stbdRudder->setCommandedPosition(rudderPosition-35);

                    digitalWrite(12, LOW);
                    check = false;
                }
                else{
                    //Serial.println("failed");
                }
            }
            else
                Serial2.flush();
        }
        else
            check = false;
    } //while

    //Temporary
    _windSensor->measureWindDirection();
    if(_windSensor->directionChanged()){
        char windStr[20];
        checksum = 0;

        sprintf(windStr, "$WND,%i,#", _windSensor->getWindDirection());
        for(size_t i=0; i<20; i++){
            if(windStr[i] != '$')
                checksum += static_cast<size_t>(windStr[i]);
            if(windStr[i] == '#')
                break;
        }
        char checkStr[20];
        itoa(checksum, checkStr, 10);
        char* inter = strcat(windStr, checkStr);
        char* windOut = strcat(inter, "|");
        Serial2.write(windOut);
        //Serial.println(windOut);
    }

    //TODO: move this to utility method and call from controllerthread.
    if(enableAutonomy != _lastAutonomyState){
        char autStr[20];
        checksum = 0;

        sprintf(autStr, "$AUT,%i,#", enableAutonomy);
        for(size_t i=0; i<20; i++){
            if(autStr[i] != '$')
                checksum += static_cast<size_t>(autStr[i]);
            if(autStr[i] == '#')
                break;
        }
        char checkStr[20];
        itoa(checksum, checkStr, 10);
        char* inter = strcat(autStr, checkStr);
        char* autOut = strcat(inter, "|");
        Serial2.write(autOut);
        //Serial.println(autOut);
    }
    _lastAutonomyState = enableAutonomy;

    /*_compass->measureHeading();
    if(_compass->headingChanged()){
        char comStr[20];
        checksum = 0;

        sprintf(comStr, "$COM,%i,#", _compass->getHeading());
        for(size_t i=0; i<20; i++){
            if(comStr[i] != '$')
                checksum += static_cast<size_t>(comStr[i]);
            if(comStr[i] == '#')
                break;
        }
        char checkStr[20];
        itoa(checksum, checkStr, 10);
        char* inter = strcat(comStr, checkStr);
        char* comOut = strcat(inter, "|");
        Serial2.write(comOut);
        Serial.println(comOut);
    }*/

    _delay_ms(50);
}
